#include <SoftwareSerial.h>
#include <ax12.h>

AX12 motor;

void setup() {
 motor = AX12();

Serial.begin (115200);
AX12::init (1000000);

byte detect;
byte num = AX12::autoDetect(&detect, 1);

Serial.print ("deteccion");
Serial.println (num, DEC);

motor.id = detect;
motor.SRL = RETURN_ALL;

  Serial.print (" ID detectado: ");
  Serial.println (detect, DEC);
  Serial.print (" delay: ");
  AX12info test = motor.readInfo (RETURN_DELAY_TIME);
  Serial.println (test.value, DEC);
  Serial.print (" error lectura: ");
  Serial.println (test.error, DEC);

delay(1000);
  motor.changeID (3);
byte num2 = AX12::autoDetect (&detect, 1);  
  Serial.print (" Nuevo ID: ");
  Serial.println (detect, DEC);
  Serial.print ("deteccion");
Serial.println (num2, DEC);

motor.id = detect;
motor.SRL = RETURN_ALL;

  Serial.print (" ID detectado: ");
  Serial.println (detect, DEC);
  Serial.print (" delay: ");
  AX12info test2 = motor.readInfo (RETURN_DELAY_TIME);
  Serial.println (test2.value, DEC);
  Serial.print (" error lectura: ");
  Serial.println (test2.error, DEC);
  
 
  
}

void loop(){
    motor.setVel (random (100, 300));
    motor.setPos (random (400, 600));
    
    delay (100);
    
    int pos = motor.getPos(); 
    int vel = motor.getSpeed(); 
    int load = motor.getLoad();
    
    Serial.println (" ");
    Serial.print (" posicion: ");
    Serial.println (pos, DEC);
    Serial.print (" velocidad: ");
    Serial.println (vel, DEC);
    Serial.print (" carga: ");
    Serial.println (load, DEC);
        
    delay (1100);
}
